/*
 * measure.c
 *
 * Created: 2012-07-26 08:45:55
 *  Author: Antrykot
 */ 



#include "../sc-p/measure.h"

u8 adc_chan = 0;
u16 adc_readout = 0;

u16 x_aku = STD_AKU_MINUS;
u16 x_pow = STD_AKU_MINUS;

u16 v_aku;
u16 v_pow;
u16 v_dc;
u16 v_temp;

u16 stn = 180;
u16 stw1 = 280;
u16 stw2 = 585;

u8 range = 0;
u8 wait_range = 0;
u8 auto_range = 0;
u8 add_meas = 0;

#ifdef MEDIA_CH4IR
void calc_l(u8 test)
{
	static u8 min_lock = 0;
/*	
	static u16 xxx = 22;
	
	xxx ++;
	if (xxx > 356)
	{
		xxx = 22;
	}
	t_pom = xxx | (1 << 14);
	return;
	*/
	if (!(PORTA & 0x01))	// if ! ZALDC
	{
		t_pom = 30;
		t_pom |= (1 << 14) | (1 << 12);	
		return;
	}
	adc_chan = 0;
	SEL_LR;
	adc_read();
	u8 minus = 0;
	float medium = adc_readout * nxa;
	if (medium > nxb)
	{
		medium -= nxb;
		min_lock = 0;
	}
	else
	{
		medium = nxb - medium;
		minus = 1;
		if (medium <= BLOCK_MINUS_L)
		{
			medium = 0;
			minus = 0;
			min_lock = 0;
		}
		
		min_lock++;
		if (min_lock < 60)		//	~3.0s		minus lockout
		{
			minus = 0;
			medium = 0;
		}
		else
		{
			min_lock = 101;
		}
		
		
	}
	medium = medium / 10;
	
	if (medium < 1)
		medium = 0;
		
	if (medium > 450 && minus == 0)
	{
		t_pom = 490;
		t_pom |= (1 << 14);
		range = 1;
		wait_range = SWITCH_TIME;
		SEL_HR;
		return;
	}
	
	if (!test)
	{
		t_pom = medium;
		t_pom |= (1 << 14);
		t_pom |= (minus << 12);
	}
	return;
}

#endif

void calc_h()
{
	if (!(PORTA & 0x01))	// if ! ZALDC
	{
		t_pom = 30;
		t_pom |= (1 << 14) | (1 << 12);
		return;
	}
	adc_chan = 0;
	SEL_HR; 
	adc_read();
	u8 minus = 0;
	float medium = adc_readout;
	medium = medium * adc_readout;
	medium *= wxa;

	medium += (float)adc_readout * wxb;
	
	if (medium >= wxc)
	{
		medium = medium - wxc;
		minus = 0;
	}
	else
	{
		medium = wxc - medium;
		minus = 1;
	}

	
	medium = medium / 100;
	
	if (medium > 1150)	//115%
	{
		medium = 1151;
	}

	static u8 wait_high = 0;

	if (auto_range)
	{
		if (medium < 40 || minus)
		{
			wait_high --;
			if (!wait_high)
			{
				SEL_LR;
				wait_range = SWITCH_TIME;
				range = 0;
			}
		}
		else
		{
			wait_high = SWITCH_TIME * 2;
		}
	}
	if (medium < 1)
		medium = 0;
	
	t_pom = medium;
	//t_pom = adc_readout / 100;
	t_pom |= (minus << 13) | (1 << 15);

	return;
}




void calc_aku()
{
	u16 result = 0;
	adc_chan = 1;
	for (u8 c = 0; c < 10; c++)
	{
		adc_read();
		result += adc_readout;
		WAITMS(1);
	}
	result /= 10;
	float aku = result * 6.10;
	aku += x_aku - STD_AKU_MINUS;
	v_aku = aku;
}

void calc_pow()
{
	u16 result = 0;
	adc_chan = 3;
	for (u8 c = 0; c < 10; c++)
	{
		adc_read();
		result += adc_readout;
		WAITMS(1);
	}
	result /= 10;
	float pow = result * 6.10;
	pow += x_pow - STD_AKU_MINUS;
	v_pow = pow;
}

void calc_dc()
{
	adc_chan = 2;
	adc_read();
	float dc = adc_readout * 6.10;
	v_dc = dc;
}

void adc_read()
{
	cli();
	
	DIN_HIG;
	WAITUS(20);
	CS_LOW;
	WAITUS(20);
	adc_clk();
	DIN_HIG;		// single ended
	adc_clk();
	DIN_LOW;		// D2
	adc_clk();
	if (adc_chan & 0x02)
	{
		DIN_HIG;
	}		
	else
	{
		DIN_LOW;
	}		
	adc_clk();
	if (adc_chan & 0x01)
	{
		DIN_HIG;
	}		
	else
	{
		DIN_LOW;
	}		
	adc_clk(); // D0
	DIN_HIG;
	adc_clk(); // empty
	adc_clk(); // empty
	adc_clk(); // empty
	
	adc_readout = 0;
	for (i8 c = 11; c >= 0; c--)
	{
		CLK_HIG;
		WAITUS(20);
		CLK_LOW;
		//WAITUS(5);
		adc_readout |= ( ( DOUT ) << c);
	}		
		
	CS_HIG;
	DIN_HIG;
	CLK_HIG;
	sei();
}

inline void adc_readbit(u8 offset)
{
	CLK_HIG;
	WAITUS(10);
	CLK_LOW;
	WAITUS(10);
	adc_readout |= ( ( DOUT ) << offset);
}

void adc_clk()
{
	CLK_LOW;
	WAITUS(20);
	CLK_HIG;
	WAITUS(20);
}


void ser_def_low()
{
		nxa = read_double(EEP_SNXA);
		nxb = read_double(EEP_SNXB);
		
		x_zn = read_word(EEP_X_SZN);
		x_cn = read_word(EEP_X_SCN);
				
		write_double(EEP_NXA,nxa);
		write_double(EEP_NXB,nxb);
		
		
		write_word(EEP_X_ZN,x_zn);
		write_word(EEP_X_CN,x_cn);

		stn = read_word(EEP_STN);


		spec_aquire = CALIB_FACT;
		
}

void ser_def_high()
{
		stw1 = read_word(EEP_STW1);
		stw2 = read_word(EEP_STW2);
		
		wxa = read_double(EEP_SWXA);
		wxb = read_double(EEP_SWXB);
		wxc = read_double(EEP_SWXC);

		write_double(EEP_WXA,wxa);
		write_double(EEP_WXB,wxb);
		write_double(EEP_WXC,wxc);
		

		x_zw = read_word(EEP_X_SZW);
		x_cw = read_word(EEP_X_SCW);
		
		write_word(EEP_X_ZW,x_zw);
		write_word(EEP_X_CW,x_cw);



}

#ifdef MEDIA_CH4IR

void cal_l_zero()
{
	adc_chan = 0;
	SEL_LR;
	adc_read();

	x_zn = adc_readout;
	
	stn = read_word(EEP_STN);
	x_cn = read_word(EEP_X_SCN);	
	
	nxa = (stn * 10) / (float)( x_cn - x_zn);
	nxb = nxa * x_zn;
	

}

void cal_l_sens(u16 sense)
{
	stn = sense;
	
	adc_chan = 0;
	SEL_LR;
	adc_read();
	
	x_cn = adc_readout;

	
	nxa = (stn * 10) / (float)(x_cn - x_zn);
	nxb = nxa * x_zn;
	
	write_word(EEP_X_CN,x_cn);
	//write_word(EEP_STN,stn);
	write_word(EEP_X_ZN,x_zn);
	write_double(EEP_NXA,nxa);
	write_double(EEP_NXB,nxb);
}
#endif

void s_cal_h_zero()
{
	adc_chan = 0;
	SEL_HR;
	adc_read();
	x0 = adc_readout / (float)10;
	x_szw = adc_readout;
	x_zw = x_szw;
}

void s_cal_h_sens1(u16 sense)
{
	adc_chan = 0;
	SEL_HR;
	adc_read();
	
	x_scw = adc_readout;
	x_cw = x_scw;
	x1 = adc_readout / (float)10;

	stw1 = sense;
	write_word(EEP_STW1,stw1);
	////// X_1 = wynik / 10
	////// St1 = sense / 100
	
}

void s_cal_h_sens2(u16 sense)
{
	adc_chan = 0;
	SEL_HR;
	adc_read();
	
	x2 = adc_readout / (float)10;
	stw2 = sense;
	
	matrix();
	
	write_word(EEP_X_SZW,x_szw);
	write_word(EEP_X_ZW,x_zw);
	
	write_word(EEP_X_SCW,x_scw);
	write_word(EEP_X_CW,x_cw);
	
	write_word(EEP_STW2,stw2);
	
	
}



void matrix()
{
	
	m1 = (float)x0 * (float)x0 * 10;
	
	m2 = x0;
	m3 = 0.1;
	m4 = (float)x1 * (float)x1 * 10;

	m5 = x1;
	m6 = 0.1;
	m7 = (float)x2 * (float)x2 * 10;
	m8 = x2;
	m9 = 0.1;
	det();
	
	float w = w_det;
	
	m1 = 0;
	m2 = x0;
	m3 = 0.1;
	m4 = stw1 / (float)100;
	m5 = x1;
	m6 = 0.1;
	m7 = stw2 / (float)100;
	m8 = x2;
	m9 = 0.1;
	det();
	float wa = w_det;
	
	swxa = (wa / w ) * 1000;
	wxa = swxa;
	
	m1 = (float)x0 * (float)x0 * 10;

	m2 = 0;
	m3 = 0.1;
	m4 = (float)x1 * (float)x1 * 10;
	m5 = stw1 / (float)100;
	m6 = 0.1;
	m7 = (float)x2 * (float)x2 * 10;
	m8 = stw2 / (float)100;
	m9 = 0.1;
	det();
	float wb = w_det;
	swxb = wb / w  * 1000;
	wxb = swxb;
	
	m1 = (float)x0 * (float)x0 * 10;
	m2 = x0;
	m3 = 0;
	m4 = (float)x1 * (float)x1 * 10;

	m5 = x1;
	m6 = stw1 / (float)100;
	m7 = (float)x2 * (float)x2 * 10;
	m8 = x2;
	m9 = stw2 / (float)100;
	det();
	float wc = w_det * -1;
	swxc = wc / w * 1000;
	wxc = swxc;
	
	
	write_double(EEP_SWXA,swxa);
	write_double(EEP_SWXB,swxb);
	write_double(EEP_SWXC,swxc);
	write_double(EEP_WXA,wxa);
	write_double(EEP_WXB,wxb);
	write_double(EEP_WXC,wxc);
	
	

}

void det()
{
w1 = m1 * m5;
w2 = w1 * m9;
w1 = w2;
w2 = m4 * m8;
w3 = w2 * m3;
w2 = w3 + w1;
w3 = m7 * m2;
w4 = w3 * m6;
w1 = w4 + w2;                                           //suma trzech
w4 = m3 * m5;
w2 = w4 * m7;
w4 = m6 * m8;
w3 = w4 * m1;
w5 = m9 * m2;
w4 = w5 * m4;
w5 = w1 - w2;
w1 = w5 - w3;
w_det = w1 - w4;
}
/*
Cal_w:

Return
*/
void cal_h_zero()
{
	adc_chan = 0;
	SEL_HR;
	adc_read();
	
	x_zw = adc_readout;
	wxa = read_double(EEP_SWXA);
	stw1 = read_word(EEP_STW1);
	x_cw = read_word(EEP_X_SCW);
	
	float tmp = ((((float)x_cw * x_cw) - ((float)x_zw * x_zw))) * wxa;
	wxb = (((stw1 * (float)100)) - tmp ) / (x_cw - x_zw);
	
	wxc = wxa * (x_zw * (float)x_zw) + (wxb * x_zw);



}

void cal_h_sens(u16 sense)
{
	adc_chan = 0;
	SEL_HR;
	adc_read();
	
	stw1 = sense;
	x_cw = adc_readout;
	
	wxa = read_double(EEP_SWXA);
	
	//write_word(EEP_STW1,stw1);
	write_word(EEP_X_ZW,x_zw);
	write_word(EEP_X_CW,x_cw);
	write_double(EEP_WXA,wxa);
	
	float tmp = ((((float)x_cw * x_cw) - ((float)x_zw * x_zw))) * wxa;
	wxb = (((stw1 * (float)100)) - tmp ) / (x_cw - x_zw);
	
	wxc = wxa * (x_zw * (float)x_zw) + (wxb * x_zw);


	
	write_double(EEP_WXB,wxb);
	write_double(EEP_WXC,wxc);
	
}


